#include <iostream>
#include <cmath>
#include <cstdio>

#include "FEMModel.h"
#include "Node.h"
#include "Tetrahedron.h"
#include "Scenario.h"
#include "Triangle.h"
#include "Setting.h"
#include "TopologicalOperation.h"
#include "ModelIO.h"
#include "Collapse.h"

#ifndef NDEBUG
#define collapse_print cout 
#else
#define collapse_print if(0) cout 
#endif

int total_degens_collapsed_count;
int triangle_collapse_count;
int sliver_collapse_count;
int spindle_collapse_count;
int cap_collapse_count;
int wedge_collapse_count;
int failed_collapse_count;

char * CollapseStatistics ()
{
	static char s[1024];

	sprintf (s,"collapsed %d degens: %d triangles, %d wedges, %d slivers, %d caps, %d spindles, failed %d\n",
		total_degens_collapsed_count, triangle_collapse_count,
		wedge_collapse_count, sliver_collapse_count,
		cap_collapse_count, spindle_collapse_count, failed_collapse_count);

	return s;
}


/*
Theoretically speaking, #define COMPLEX would not alter the shape of
the mesh. Unfortunately, it causes even more degeneracies (many of
them hopeless, it seems?) sometimes.

It seems that the selection routines make wrong choices for collapse
routines, causing the subdivision to happen close to either node of
the edge, thus creating more degens.

Either that , or I don't know. This whole meshing business is
hopeless, essentially.
*/

// #define COMPLEX

/*

Find MP1 and MP2 from P0,P1 and Q0, Q1 respectively such that

MP1 - MP2 is perpendicular to P0-P1, Q0-Q1.

*/
void FindMidpoints (Vector3 *mp1, Vector3 *mp2, Vector3 p[], Vector3 q[])
{
#ifdef COMPLEX
	Vector3 b0 = p[1] - p[0];
	Vector3 b1 = q[1] - q[0];
	Vector3 n = Vector3::cross(b0, b1);
	Vector3 bvecs[] = {b0, b1, n};

	Vector3 rhs = p[0] - q[0];
	Matrix3 basis,inv ;
	basis.SetVectors (bvecs[0], bvecs[1], bvecs[2]);
	Matrix3::InvertTo (inv,basis);
	Vector3 sol = inv* rhs;

	sol (0) = - sol(0); 
	if ( ! (0 <= sol(0) && sol(0) <=  1
		&&0 <= sol(1) && sol(1) <=  1))
	{
		collapse_print << "************\n\nFind_midpoint: ugh: " << sol << endl;
		assert (false);
	}

	collapse_print << "sol0, sol1: " << sol(0) << " " << sol(1) <<endl;

	*mp1 = p[0] + b0 * sol(0);
	*mp2 = q[0] + b1 * sol(1);
#else
	*mp1 = 0.5*(p[0]+ p[1]);
	*mp2 = 0.5*(q[0]+ q[1]);  
#endif
}

Vector3 ProjectPointOnLine (Vector3 l[], Vector3 p)
{
#ifdef COMPLEX
	Vector3 d = l[1] - l[0];
	d/=d.Length ();
	Real factor = ((p -l[0]) * d);

	if (! (0.0 <= factor && factor <= 1.0))
	{
		collapse_print << "************\n\nProject point on midline: " << factor << endl;

		if (factor < 0.0)
			factor = 0.0;
		else if (factor> 1.0)
			factor = 1.0;
	}

	collapse_print << "\nfactor: " <<factor <<endl;

	return l[0] + d * factor;
#else
	return 0.5 *(l[0] + l[1]);
#endif
}

/*
Collapse T along its shortest edge.
*/
bool TryCollapseWedge (FiniteElementModel*fem, Tetrahedron *t,
					Real threshold )
{
	collapse_print << "Collapsing wedge " << t->Number ()  << " ... " <<flush;
	Simplex min;
	Real minlength = 1e8;
	for (int i=4; i--;)
		for (int j = i; j--;)
		{
			Node *n[] = {t->GetNode (i), t->GetNode (j)};

			Real l = (n[0]->Location (false) - n[1]->Location (false)).Length ();

			if (l < minlength)
			{
				min = Simplex (EDGE, n, 0);
				minlength = l;
			}
		}

		Vector3 midpoint = 0.5* (min.GetNode (0)->Location (false) + min.GetNode (1)->Location (false));

		bool succ =  TryCollapseEdge (fem, min, t, 0, midpoint);

		collapse_print << "done"<<endl;

		if (succ)
			wedge_collapse_count++;

		return succ;
}

/*
ENTRY POINT

Check all tetrahedra in FEM, cleaning degenerate cases. Loop until
we do a pass with no degeneracies

RETURN:

whether we went down to 0 degeneracies.

TODO:

We should really do the loop, while remembering which tetrahedra are
hopeless. If there is a mix, the hopeless and non-hopeless
degeneracies might all be junked because the number of degens
increased during one step.

*/
bool CollapseDegeneracies (FiniteElementModel*fem)
{
	Real max = GetNumberSetting ("collapse-degeneracy-passes");
	collapse_print<< "Searching for degens ..." <<flush; 


	int last_degens_detected = 0;
	int iter = 0;

	Real aspect_ratio_threshold = GetNumberSetting ("tetra-aspect-ratio-threshold");
	Real dihedral_angle_threshold = GetNumberSetting ("dihedral-angle-threshold");
	Real triangle_aspect_threshold = GetNumberSetting ("triangle-aspect-ratio-threshold");

	do
	{
		int degens_detected =0;
		if (iter >= max)
			break ;

		iter ++;
		if (last_degens_detected != 0)
			collapse_print << " found " << last_degens_detected << " degens \n"
			<< "******* trying another time ********\n\n";

		LinkArray<Tetrahedron> todo (fem->TetrahedronArray ());
		for (int i =todo.Size (); i--; )
		{
			Tetrahedron* tet = todo[i];
			if (!tet->Valid ())
				continue;

			Real a  = AspectRatio (tet, false);
			if (a > aspect_ratio_threshold)
				continue;
			collapse_print << "tet "  << tet->Number () << "degenerate: aspect "<< a << flush;
			bool tri_success  =TryCollapseTriangles(fem, tet, triangle_aspect_threshold);
			if (tri_success)
			{
				degens_detected ++;
				continue;
			}

			/*
			try_collapse_triangles () might fail halfway; 
			*/
			if (!tet->Valid ())
				continue;

			Real m, M ;
			MinmaxDihedralAngle (tet, &m, &M, false);

			collapse_print << " tetra degenerate: mindih " <<  m << " max dih " << M << endl;

			bool (*fp) (FiniteElementModel*, Tetrahedron*,Real);

			fp = (M < dihedral_angle_threshold  * M_PI) ? TryCollapseWedge : TryCollapseLargeDihTetra;

			if ((*fp) (fem,tet, dihedral_angle_threshold))
				degens_detected ++;
			else
				failed_collapse_count ++;

		}
		if (degens_detected == 0)

			break;

		if ( last_degens_detected && degens_detected >= last_degens_detected)
		{
			collapse_print << "uhoh. Increasing # of degens!"<<endl;
			return false;
		}

		total_degens_collapsed_count += degens_detected;
		last_degens_detected = degens_detected ;
	}
	while (1);



	printf ("done\n");
	return true;
}

Real EdgeLength (Simplex e)
{
	assert (e.Type () == EDGE);
	return ( e.GetNode (0)->Location (false) -  e.GetNode (1)->Location (false) ).Length ();
}

/*
Collapse a spindle: find the edge opposite the large dihedral, and
split it in half.
*/
bool TryCollapseSpindle (FiniteElementModel*fem, Tetrahedron* t, int a, int b)
{
	collapse_print << "collapsing spindle " << t->Number ()  << "..." << flush;

	int p[2];
	int j = 0;

	for (int i = 4; i--;)
		if (i != a && i!= b)
			p[j++] = i;

	assert (j == 2);

	Simplex e (GetTriangleIntersection (t, p[0], p[1]));
	Simplex orig = t->GetSet ();

	/*
	Determine the point on E exactly opposite the flat dihedral angle
	specced by A,B */
	Vector3 edge_locs[]
	= {e.GetNode(0)->Location(false), e.GetNode(1)->Location(false)};

	Simplex flat_angle_edge (GetTriangleIntersection (t, a, b));
	Vector3 opposite_point = ProjectPointOnLine (edge_locs, flat_angle_edge.GetNode (0)->Location (false));


	Node*  newnod = TrySplitEdge (fem, e, t, opposite_point);
	if (newnod)
	{
		Tetrahedron * t0 = dynamic_cast<Tetrahedron*> (fem->FindObject (orig.GetSubstituted (e.GetNode (0), newnod)));
		Tetrahedron * t1 = dynamic_cast<Tetrahedron*> (fem->FindObject (orig.GetSubstituted (e.GetNode (1), newnod)));

		collapse_print <<  " t0 a " << AspectRatio (t0,false) << " t1 a = " << AspectRatio (t1,false) << endl;
	}

	spindle_collapse_count ++;

	printf (" done \n");
	return newnod!=NULL; 
}

// #define SHOW_COLLAPSE
#ifdef SHOW_COLLAPSE 
/*
We collapse a sliver by splitting it in 4, and collapsing the newly
created diagonal.

TODO: should base choice of collapse_sliver and collapse_cap on
whether the result of project_point_on_line() lies within the line
segment, not on dihedral angles.
*/
bool TryCollapseSliver (FiniteElementModel*fem, Tetrahedron *t,
					 int a1, int a2, int b1, int b2)
{
	collapse_print << "collapsing sliver " << t->Number ()  << "..."<< flush; 
	Simplex e1 (GetTriangleIntersection (t, a1, a2));
	Simplex e2 (GetTriangleIntersection (t, b1, b2));

	Vector3 ps1[] = {e1.GetNode(0)->Location(false), e1.GetNode (1)->Location(false)};
	Vector3 ps2[] = {e2.GetNode(0)->Location(false), e2.GetNode (1)->Location(false)};

	Vector3 mp1, mp2;
	FindMidpoints (&mp1, &mp2, ps1, ps2);

	Simplex tetset = t->GetSet();
	Node *ns[2];

	ns[0]= TrySplitEdge (fem, e1, t, mp1);

	Tetrahedron *t2 = dynamic_cast<Tetrahedron*>
		(fem->FindObject (tetset.GetSubstituted (e1.GetNode (0), ns[0])));

	tetset = t2->GetSet();
	ns[1] =  TrySplitEdge (fem, e2, t2, mp2);

	return true;
}

#else

bool TryCollapseSliver (FiniteElementModel*fem, Tetrahedron *t,
					 int a1, int a2, int b1, int b2)
{
	collapse_print << "collapsing sliver " << t->Number ()  << "..."<< flush; 
	Simplex e1 (GetTriangleIntersection (t, a1, a2));
	Simplex e2 (GetTriangleIntersection (t, b1, b2));

	Vector3 ps1[] = {e1.GetNode(0)->Location(false), e1.GetNode (1)->Location(false)};
	Vector3 ps2[] = {e2.GetNode(0)->Location(false), e2.GetNode (1)->Location(false)};

	Vector3 mp1, mp2;
	FindMidpoints (&mp1, &mp2, ps1, ps2);

	Simplex tetset = t->GetSet();
	Node *ns[2];

	ns[0]= TrySplitEdge (fem, e1, t, mp1);

	Tetrahedron *t2 = dynamic_cast<Tetrahedron*>
		(fem->FindObject (tetset.GetSubstituted (e1.GetNode (0), ns[0])));

	tetset = t2->GetSet();
	ns[1] =  TrySplitEdge (fem, e2, t2, mp2);

	bool succ = false; 
	if (ns[0] && ns[1])
	{
		Simplex e (EDGE, ns, 0);

		tetset = tetset.GetSubstituted (e2.GetNode (0), ns[1]);
		t2 = dynamic_cast<Tetrahedron*> (fem->FindObject (tetset));

		Vector3 mp = 0.5 *  (mp1 + mp2);

		succ = TryCollapseEdge (fem, e, t2, 0, mp);
	}

	if (succ)
		sliver_collapse_count++;

	collapse_print << " done; succ =  "  << succ <<endl;
	return succ;
}
#endif

/*
Junk a cap: subdivide the triangle opposite the top of the cap, and
collapse the diagonal created thus.  */
bool TryCollapseCap (FiniteElementModel*fem,Tetrahedron* tet, Array<int> dihs)
{
	collapse_print << "Collapsing cap " << tet->Number ()  << " ... " << flush;
	Triangle* mintri = 0;
	Node * minnode = 0;
	Real minheight = 1e8;
	bool dyn = false;
	for (int i = 0; i < 4 ; i++)
	{
		Triangle * t = tet->GetTriangle (i);
		Node  *n = tet->GetNode (i);
		Real o1 = t->Normal (dyn) *
			(t->GetNode (0)->Location (dyn) -  n->Location (dyn));

		if (fabs (o1) < minheight)
		{
			minheight = fabs(o1);
			mintri = t;
			minnode = n;
		}
	}

	Vector3 split_pos = minnode->Location (false);
	Vector3 normal = mintri->Normal(false);
	split_pos = split_pos
		+ normal * (mintri->GetNode(0)->Location(false) - split_pos) * normal;

	Simplex triset = mintri->GetSet();
	Simplex tetset = tet->GetSet ();
	Node *newnod = TrySplitTriangle (fem, mintri, split_pos);

	TopologicalObject * splitted_tet
		= fem->FindObject (tetset.GetSubstituted (triset.GetNode (0), newnod));

	Node *ens[] = {newnod, minnode};
	Simplex edge (EDGE, ens, 0);

	bool succ = TryCollapseEdge (fem, edge, dynamic_cast<Tetrahedron*> (splitted_tet), 0, Centroid (edge, false));

	if (succ)
		cap_collapse_count ++;

	printf ("done \n");
	return succ;
}

/*
Collapse a tetrahedron T: check whether it is a sliver, spindle or
cap and call an appropriate routine..

PRE: T has some large dihedral angle */
bool TryCollapseLargeDihTetra (FiniteElementModel*fem, Tetrahedron * t, Real threshold)
{
	Array<int> dihs;

	for (int i=0; i < 4; i++)
		for (int j = i+1; j < 4; j++)
		{
			Real a = DihedralAngle (t, i,j, false);
			if (a > threshold * M_PI)  
			{
				dihs.Push (i);
				dihs.Push (j);
			}
		}

		if (dihs.Size () == 2)
		{
			return TryCollapseSpindle (fem, t, dihs[0], dihs[1]);
		}
		else if (dihs.Size () == 4)
		{
			return TryCollapseSliver (fem, t, dihs[0], dihs[1],
				dihs[2], dihs[3]);
		}
		else if (dihs.Size () == 6)
		{
			return TryCollapseCap (fem, t, dihs);
		}
		else
		{
			printf ("Huh??? \n");
			assert (false);
		}

		return true;
}
/*
Compute the aspect ratio of  a triangle: minimum height / maximum edge. 
*/
Real AspectRatio (Triangle* t)
{
	Vector3 v1 = t->GetNode (0)->Location (false) - t->GetNode (2)->Location (false);
	Vector3 v2 = t->GetNode (1)->Location (false) - t->GetNode (2)->Location (false);

	Real area= Vector3::Cross (v1, v2).Length ();;
	Real  maxl = 0;
	for (int i=0;  i < 3; i++)
	{
		Real l = EdgeLength (t->GetSet ().GetSubset (i));
		maxl = maxl >l?maxl:l;
	}

	return (2 * area / maxl) / maxl;
}

/*
split T along its longest edge, then collapse the newly created edge */
bool TryCollapseTriangle (FiniteElementModel*fem, Triangle* tri)
{
	collapse_print << "collapsing triangle " << tri->Number () << " ... "  <<flush;
	Simplex maxset, triset = tri->GetSet ();

	Real maxl = 0.;
	Node * flatnode = 0;
	for (int i=0;  i < 3; i++)
	{

		Real l = EdgeLength (triset.GetSubset (i));
		if (l >  maxl)
		{
			maxset = triset.GetSubset (i);
			maxl =l;
			flatnode = tri->GetNode (i);
		} 
	}

	Vector3 line []
	= {maxset.GetNode(0)->Location (false),
		maxset.GetNode(1)->Location (false)};
	Vector3 mp = ProjectPointOnLine (line, flatnode->Location (false));

	Tetrahedron * tet = tri->Tetra  ();
	Simplex tetset = tet->GetSet ();

	Node * newnod = TrySplitEdge (fem, maxset, tet, mp);
	assert (newnod);

	tet = dynamic_cast<Tetrahedron*> (fem->FindObject (tetset.GetSubstituted (maxset.GetNode (0), newnod)));

	Node *tocollapse[] = { newnod, flatnode};
	Simplex collapse_edge (EDGE, tocollapse, 0);
	bool succ = TryCollapseEdge (fem, collapse_edge, tet, 0,Centroid (collapse_edge,false));

	if (succ)
		triangle_collapse_count ++;

	collapse_print << " succ = " << succ << ". Done" <<endl;
	return succ;
}


/*
Check every triangle of T for degeneracy (judged using TRI_ASPECT), and collapse. )
*/
bool TryCollapseTriangles (FiniteElementModel*fem, Tetrahedron* tet,  Real tri_aspect)
{
	bool succ = false; 
	for (int i=4; !succ && i--;)
	{
		Triangle *tri = tet->GetTriangle (i);

		/*
		we must check TRI for validity, since try_collapse_triangle ()
		typically splits TET, invalidating both TRI and TET.  */
		if (tri->Valid ()
			&& AspectRatio (tri) < tri_aspect)
			succ = TryCollapseTriangle (fem, tri);
	}
	return succ;
}
